1.2 点云数据处理

本文采用FARO高精度静态三维激光扫描仪进行环境点云数据采集，首先设置合理的扫描分辨率对环境进行多次扫描，相邻测站之间摆放3个及以上共视标靶球，并测量标靶球的绝对位置坐标；然后通过FARO扫描仪自带点云处理软件Scene对多站点云进行拼接处理，生成一个完整的环境点云；之后根据测量的标靶球的绝对位置坐标及其在拼接点云中的局部坐标求解坐标转换七参数，通过七参数将点云转换至绝对坐标系下。在建立二维栅格地图前需对场景中动态物体及高度较大物体上的激光点进行剔除，仅保留实际应用中在车载激光雷达扫描高度范围内的点。

1.3 栅格地图表示与构建

栅格地图因其直观、计算量小的优点，近年来逐渐成为移动机器人领域使用最广泛的地图表示方法 [5] 。栅格地图根据一定的坐标间距将环境分隔为一个个栅格，用 p ( s =1)表示每个栅格被障碍物占据(occupied)的概率，如图 1所示，本文对每个栅格预定义占据概率值为0.1、0.3、0.6和0.9。栅格地图中的每个栅格初始概率值为0.1，若某个栅格中存在被投影的激光点，则该栅格被赋值为0.9，与其相邻的栅格被赋值为0.6，与其相隔一个栅格的所有栅格被赋值为0.3，若上述相邻栅格也与其他有激光点的栅格相邻，其概率值相应增加。

构建栅格地图时采用高斯投影将所有点云坐标转换为高斯平面坐标( x, y, h )，然后根据点云在高斯平面的坐标确定栅格地图边界，将每个激光点对应至相应的栅格，最后根据上述规则计算出所有栅格的概率值并存储。

2 UGV定位与导航方法

在已有场景先验地图的情况下，基于激光扫描的匹配定位需要载体合适的初始位置与姿态，此后使用高斯-牛顿搜索方法更新载体位姿。

2.1 初始位姿计算

本文以 ξ =( x, y, φ )表示LiDAR在世界坐标的平面坐标与航向，激光点在世界坐标系中的坐标与在LiDAR坐标系下坐标的转换关系为

(1)

式中, ，为激光点在LiDAR坐标系下坐标; s i ( ξ )为根据LiDAR位姿计算的激光点在世界坐标系下的坐标，将其投影至对应的栅格即可计算概率值。

极大似然估计是一种根据观测值估计模型参数的方法，用于室内激光匹配定位中可获得分米级定位精度 [2] ，在进行匹配定位前，通过似然函数式(2)计算首帧激光点在栅格地图 M 搜索窗口 内的似然性，通过滑窗搜索得到LiDAR的粗略初始位姿满足式(3)。

hj (2)

(3)

单一分辨率栅格地图匹配定位无法兼顾定位精度和计算效率，为提高搜索效率并减小优于低分辨率造成的定位精度损失，本文采用多分辨率栅格地图进行匹配定位，即先在低分辨率地图上进行搜索，缩小位姿搜索区域后，逐步在高分辨率子图层上搜索，获得LiDAR较为准确的位姿初值。

2.2 扫描帧-地图匹配定位

在激光SLAM中，扫描匹配的主要任务是基于环境的外形，根据相邻两帧或多帧扫描数据之间的关系，估计载体的刚性运动。目前主要的匹配方法有迭代最近点(iterative closest point，ICP)及其变体，相关性扫描匹配(correlation scan match，CSM)等，相邻帧匹配的定位方法估计的是载体的旋转和平移量，即位姿的变化量进行位姿推算，其定位误差累积是不可避免的。文献[15]提出了一种基于高斯-牛顿搜索的扫描帧与已有地图匹配的定位方法，即采用高斯-牛顿方法迭代计算LiDAR位姿的局部最优解。该方法对位姿初值敏感，当给定准确的初始位姿时，扫描帧与地图匹配的定位方法更为高效且误差不累积。

待求解的LiDAR位姿及激光点坐标如式(1)所示。 为根据激光点坐标 计算的地图占据概率值，待求解的载体姿态应满足以下条件

(4)

对于给定位姿初值 ξ ，匹配定位的目标即找到初值误差Δ ξ 的最优估计使得

(5)

趋近0。采用高斯-牛顿法迭代计算至Δ ξ 小于指定阈值，由于地图概率值为离散值，即 不连续，因此在对 进行泰勒级数展开时，激光点概率值梯度为

(6)

然后根据其最近4个整数坐标概率值进行双线性插值计算。

获得LiDAR初始位姿后，在相邻LiDAR扫描帧之间使用采样频率更高的惯性测量单元(inertial measurement unit, IMU)数据进行航位推算(在使用点云匹配定位系统前对LiDAR、IMU及车体进行标定)，将IMU推算的LiDAR位姿作为初值进行高斯-牛顿搜索能有效减少迭代计算次数和减小匹配结果协方差。

3 试验与结果分析

为验证基于三维激光扫描预建栅格地图及LiDAR扫描的二维地图匹配定位方法的有效性，本文利用车载LiDAR、IMU与GNSS接收机进行了实时定位试验，并将二维匹配定位结果与INS/RTK组合定位结果进行了比对。

3.1 试验设计

试验场地位于某开放园区内，园区非常空旷，试验时布设了26组水马围栏作为障碍物。本文使用FARO FOCUS系列三维扫描仪进行扫描建图，其量程为0.6~150 m，测距精度为1 mm。所采用IMU为SBG Ellipse2-N，航向测量精度为0.5°，RTK定位采用Trimble高精度定位板卡Bd982，基站架设于试验场地内。采用Robosense 16线LiDAR，量程为0.2~150 m，测距精度为2 cm。试验时LiDAR与GNSS天线安装在车顶，惯导系统安装在车内，仅采用其水平扫描线进行匹配定位。试验流程、试验设备及环境如图 2和图 3所示。

3.2 点云匹配定位结果分析

试验中对试验场地进行了三维激光扫描，并在CloudCompare软件中对原始点云进行处理，对试验场地外、地面点及动态障碍物点进行了剔除，对场地建立最大分辨率5 cm的栅格地图用于匹配定位。试验中激光雷达安装于载体前端，IMU安装于载体中部，试验前已对IMU和LiDAR进行了标定。单站三维扫描数据如图 4所示，平面地图与定位轨迹如图 5所示。

试验时载体首先静止一小段时间，以获得较为准确的位姿初值，随后低速在试验场地绕“8”字运动，载体航迹如图 6所示。图 7展示了RTK/INS组合与LiDAR扫描匹配定位航迹，当载体在障碍特征较丰富的场地中低速运动时，LiDAR扫描匹配定位结果与RTK/INS组合定位结果符合得较好，图 7呈现了二者定位结果坐标差异。表 1给出了三维扫描建图时LiDAR扫描匹配定位的误差统计，从表 1可以看出，LiDAR扫描匹配定位平面误差均方根为5.871 4 cm，均值为3.905 2 cm，最大值为1.376 5 dm。

表 1误差统计结果

分析上述试验结果可以看出，尽管三维扫描所建立的栅格地图分辨率为5 cm，但由于本文所采用的方法中地图概率值计算采取了双线性插值，因此LiDAR匹配定位结果与RTK/INS组合定位结果差异整体优于1 dm，且在极大似然搜索提供的位姿初值误差较大的情况下也能较快收敛。在低速运行且环境特征较好的情况下，三维扫描建图结合LiDAR扫描匹配定位精度可达到厘米级且定位结果连续可靠。

4 结语

本文针对传统激光扫描建立的地形图不适用于移动机器人、LiDAR扫描帧与帧之间匹配定位误差累积严重等问题，采取了一种预先获取驾驶场景激光点云数据进行二维栅格地图构建，并基于高斯-牛顿法计算载体位姿的方法，运用极大似然估计获取载体初始位姿并以惯性导航系统计算位姿作为高斯-牛顿搜索的精确初值。试验结果表明，三维扫描建图结合LiDAR扫描匹配定位方法能实现厘米级连续定位，具有较好的稳定性，满足移动机器人自动驾驶需求。

王一文, 钱闯, 唐健, 等. 预建高精度地图的封闭区域UGV自动驾驶导航定位[J]. 测绘通报，2020(1)：21-25. DOI: 10.13474/j.cnki.11-2246.2020.0005.

WANG Yiwen, QIAN Chuang, TANG Jian, et al. UGV's automated driving navigation and localization in closed areas with pre-established high precision planar map[J]. Bulletin of Surveying and Mapping, 2020(1): 21-25. DOI: 10.13474/j.cnki.11-2246.2020.0005. 返回搜狐，查看更多